#include<stdio.h>
#include<Mesh_IO/Mesh_IO.h>

#include<Arcroll.h>
#include<Mesh/libcell_iterator.h>

#include<tool/libcell_tools_kd_node.h>
#include<time.h>
#include<macros.h>

#include<tool/libcell_tools_view.h>
#include<tool/viewer_tools.h>

#define Matrix4x4 Viewer_Matrix4x4_

RB_Tree_func_declare(double)

RB_Tree_func(double)
#include<map>
using namespace std;



void add_default_somethings(Viewer_World* vw)
{
    Node* n=vw->create_something(vw,"Camera");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Camera* vc=(Viewer_Camera*)(vs->evolution);
    vc->is_using=1;
    Matrix4x4 *p=Projection(M_PI/3.0f,
        (float)(vw->g_info->resolution[0])/(float)(vw->g_info->resolution[1]),0.5f,200.0f);
    p->copy_data(vc->Proj,p);
    Matrix4x4_free(p);
    free_node(n);
    
    n=vw->create_something(vw,"Intera");
    vs=(Viewer_Something*)(n->value);
    Viewer_Intera*vi=(Viewer_Intera*)(vs->evolution);
    Viewer_Arcroll*va=(Viewer_Arcroll*)malloc(sizeof(Viewer_Arcroll));
    viewer_arcroll_init(va);
    va->mc=vc;
    va->vw=vw;
    vi->representation=(void*)va;
    vi->cursor_position_callback=viewer_Arcroll_cursor_position_callback;
    vi->scroll_callback=viewer_Arcroll_scroll_callback;
    vi->mouse_button_callback=viewer_Arcroll_mouse_button_callback;
    vi->key_callback=viewer_Arcroll_key_callback; 
    free_node(n);
}
void test_show_mesh_lines(Viewer_World* vw,Mesh* m)
{
    Node* n=vw->create_something(vw,"Edges");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    ve->Data_rows=m->num_v(m);
    ve->Data_index_rows=m->num_f(m);
    get_lines_data_from_2dim_cell(m,&(ve->Data),&(ve->Data_index));
    ve->color_rows=ve->Data_index_rows;
    ve->set_color(ve,0.9,0.5,0.5,1.0); 
    free_node(n);

}
void test_show_mesh_cells(Viewer_World* vw,Mesh*m)
{
    Node* n=vw->create_something(vw,"Faces");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Faces* vf=(Viewer_Faces*)(vs->evolution);
     
    vf->Data_rows=m->num_v(m);
    vf->Data_index_rows=m->num_c(m);
    get_data_from_2dim_cell(m,&(vf->Data),&(vf->Data_index));
    

    vf->color_rows=vf->Data_index_rows;
    vf->set_color(vf,0.4,0.6,0.7,1.0);
    vf->normal_rows=vf->Data_index_rows;
    vf->compute_normal(vf);
    
    free_node(n); 

}



static inline double *out_product(double *a,double*b)
{
    double* re=(double*)malloc(sizeof(double)*3);
    
    re[0]=a[1]*b[2]-a[2]*b[1];
    re[1]=-a[0]*b[2]+a[2]*b[0];
    re[2]=a[0]*b[1]-a[1]*b[0];
    return re; 
}
static inline double inner_product(double* a,double* b)
{
     return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
static inline int normalize(double *a)
{
    double norm=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    
    norm=SAFE_SQRT(norm);
    //1e-12 doen't affect the result
    if(norm==0||fabs(norm)<1e-11)
    {
        return 0;
    } 
    a[0]/=norm;a[1]/=norm;a[2]/=norm;
    return 1;

}


static inline double*get_triangle_plane_equation(double* tr1,double* tr2,double*tr3)
{
    double a[3]={0},b[3]={0};
    a[0]=tr2[0]-tr1[0];
    a[1]=tr2[1]-tr1[1];
    a[2]=tr2[2]-tr1[2];

    b[0]=tr3[0]-tr2[0];
    b[1]=tr3[1]-tr2[1];
    b[2]=tr3[2]-tr2[2];
    double * n=out_product(a,b);
    if(!normalize(n))
    {
        free(n);
        return NULL;
    }
    double * re=(double*)malloc(sizeof(double)*4);
    re[3]=-(n[0]*tr1[0]+n[1]*tr1[1]+n[2]*tr1[2]);
    re[0]=n[0];re[1]=n[1];re[2]=n[2];
    // printf("test :%lf\n",re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    
    free(n); 
    return re; 
}


Matrix4x4* get_matrix_q_from_cell(template_c* c)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_float(re);
    double * plane=get_triangle_plane_equation(c->vertices[0]->point,c->vertices[1]->point,c->vertices[2]->point);
    float* data=(float*)(re->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=plane[i]*plane[j];
        }
    }
    //printf("%lf\n",inner_product(plane,plane));
    SAFE_FREE(plane);
    return re; 
}

Int_RB_Tree* compute_matrix_q_every_cell(Mesh* m)
{
    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        Matrix4x4* mat=get_matrix_q_from_cell(quote(cit));
        re->insert(re,quote(cit)->id,mat); 
    }

    return re;
}

//m= m1+m2
Matrix4x4* matrix_add(Matrix4x4* m1,Matrix4x4* m2)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_float(re); 
   
    float* data=(float*)(re->data),*data1=(float*)(m1->data),*data2=(float*)(m2->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=data1[i*4+j]+data2[i*4+j];
        }
    }
    return re;
}


Matrix4x4 * get_matrix_q_from_vertex(template_v* v,Int_RB_Tree* tree)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_float(re);
    re->zero(re);
    float* data=(float*)(re->data);
    for(Node* nit=v->cells;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        Matrix4x4* m=(Matrix4x4*)tree->find(tree,c->id);
        float* data1=(float*)(m->data); 
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                data[i*4+j]+=data1[i*4+j];
            }
        }
    }

    return re;
}

static inline int is_edge_legacy_to_collapse(Mesh* m,template_f* f)
{
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    Node*n1= m->vv_begin(m,*v1),*n2=m->vv_begin(m,*v2);
    Node* n=node_intersection(n1,n2);
    int size=node_size(n);

    free_node(n);free_node(n1);free_node(n2);

    return size;
}

//每个点的矩阵属性

Int_RB_Tree* qem_init_qi(Mesh * m)
{

    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);

    Int_RB_Tree* tree= compute_matrix_q_every_cell(m);
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        Matrix4x4* m= get_matrix_q_from_vertex(quote(vit),tree);
        re->insert(re,quote(vit)->id,m);
    }
    for(auto it=tree->begin(tree);it.it!=NULL;it++)
    {
        Matrix4x4_free((Matrix4x4*)(it.second));
    }
    int_rb_tree_free(tree);
    return re;
}

static inline double*  compute_qem_one_edge_position(Mesh* m,template_f* f,Int_RB_Tree *tree)
{
    double* p=NULL;
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    Matrix4x4* m1=(Matrix4x4*)tree->find(tree,v1->id);
    Matrix4x4* m2=(Matrix4x4*)tree->find(tree,v2->id);
    Matrix4x4* m3=matrix_add(m1,m2);
    float* data=(float*)(m3->data);
    data[12]=0;data[13]=0;data[14]=0;data[15]=1;
    Matrix4x4* mi=m3->inverse(m3); 
    if(mi!=NULL)
    {
        float* datami=(float*)(mi->data);
        p=(double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        p[0]=datami[0*4+3];p[1]=datami[1*4+3];p[2]=datami[2*4+3];
        Matrix4x4_free(mi);
    } 
    else
    {
        p= (double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        p[0]=(v1->point[0]+v2->point[0])/2.0;
        p[1]=(v1->point[1]+v2->point[1])/2.0;
        p[2]=(v1->point[2]+v2->point[2])/2.0;
    } 
    Matrix4x4_free(m3);
    return p;
}

Int_RB_Tree* compute_qem_all_edge_position(Mesh* m,Int_RB_Tree* tree)
{
    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    for(auto fit=m->f_begin(m);fit!=m->f_end(m);fit++)
    {
        int size=is_edge_legacy_to_collapse(m,quote(fit));
        if(size==2)
        {
            double* p=compute_qem_one_edge_position(m,quote(fit),tree);
            re->insert(re,quote(fit)->id,p);  
        } 
        else if(size>2)
        {
           // printf("is not legacy\n");
        }
        else
        {
            //printf("is boundary\n"); 
        }
    } 
    return re;
}

double compute_xTAx(Matrix4x4* Q,double* p)
{
    float* data=(float*)(Q->data);

    double p1[4]={0,0,0,0},p2[4]={p[0],p[1],p[2],1};
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            p1[i]+=data[i*4+j]*p2[j];
        }
    }
    return (p2[0]*p1[0]+p2[1]*p1[1]+p2[2]*p1[2]+p2[3]*p1[3]);
}

//tree是每个边对应的点坐标
//tree1是每个点对应的q矩阵

static inline double qem_compute_one_edge_cost(template_f*f ,Int_RB_Tree*tree,Int_RB_Tree* tree1)
{
    double* p=(double*)(tree->find(tree,f->id));
    Matrix4x4* m1=(Matrix4x4*)(tree1->find(tree1,f->vertices[0]->id));
    Matrix4x4* m2=(Matrix4x4*)(tree1->find(tree1,f->vertices[1]->id));
    Matrix4x4 *m3=matrix_add(m1,m2);
    double re= compute_xTAx(m3,p);
    Matrix4x4_free(m3);

    return re; 
}
//返回的二叉树是
//key: cost 
//value: face's id 
//tree是每个边对应的点坐标
//tree1是每个点对应的q矩阵

map<double,int>* qem_get_edge_collapse_cost(Mesh* m ,Int_RB_Tree* tree,Int_RB_Tree* tree1)
{ 
    map<double,int>*re=new map<double,int>;
    for(auto it=tree->begin(tree);it.it!=NULL;it++)
    {
        template_f* f=m->get_facep(m,it.first);
        double* p=(double*)(it.second);
        Matrix4x4* m1=(Matrix4x4*)(tree1->find(tree1,f->vertices[0]->id));
        Matrix4x4* m2=(Matrix4x4*)(tree1->find(tree1,f->vertices[1]->id));
        Matrix4x4* m3=matrix_add(m1,m2);
        double cost=compute_xTAx(m3,p);
        (*re)[cost]=it.first; 
        Matrix4x4_free(m3);
    }
    return re;
}

//tree_v 是每个点对应的矩阵
//tree_e 是每个边对应的三维坐标
//tree_cost是每个cost对应的边
void qem_collapse_edge(Mesh *m ,template_f* f,Int_RB_Tree* tree_v,Int_RB_Tree*tree_e,map<double,int>* tree_cost)
{ 
    printf("begin qem_collapse_edge\n");
    template_v* v1=f->vertices[0],*v2=f->vertices[1],
        *n_v=m->create_vertexv(m,((double*)(tree_e->find(tree_e,f->id))),3); 
    Matrix4x4* m1=(Matrix4x4*)(tree_v->find(tree_v,v1->id));
    Matrix4x4* m2=(Matrix4x4*)(tree_v->find(tree_v,v2->id));
    Matrix4x4* m3=matrix_add(m1,m2);
    tree_v->erase(tree_v,v1->id);
    tree_v->erase(tree_v,v2->id); 

    tree_v->insert(tree_v,n_v->id,m3);
    Matrix4x4_free(m1);Matrix4x4_free(m2);
    Int_RB_Tree* cells=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(cells); 
    Int_RB_Tree* boundary_hfaces=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(boundary_hfaces);
    for(auto vcit=m->vc_begin(m,*v1);vcit!=m->vc_end(m,*v1);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit)); 
    }
    for(auto vcit=m->vc_begin(m,*v2);vcit!=m->vc_end(m,*v2);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit));  
    }
    Int_RB_Tree* faces=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(faces);
    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        for(auto chfit=m->chf_begin(m,*c);chfit!=m->chf_end(m,*c);chfit++)
        {
            template_hf* hf=quote(chfit);
            if(hf->vertices[0]!=v1&&hf->vertices[0]!=v2&&hf->vertices[1]!=v1&&hf->vertices[1]!=v2)
            {
                boundary_hfaces->insert(boundary_hfaces,hf->id,hf);
            }
            else
            {
                faces->insert(faces,hf->face->id,hf->face);
            }
        }  
    }

    for(auto it=faces->begin(faces);it.it!=NULL;it++)
    {
        //update tree_e
         Int_RB_Tree_Node* irtn=int_rb_tree_find(tree_e,it.first);
         if(irtn!=NULL)
         {
            //printf("cuowu\n");
            free(irtn->data);
            int_rb_tree_delete_node(tree_e,irtn);
         }  
    }
    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        m->delete_cell(m,*c,true);
    }

    printf("fdsfsdf\n");
    for(auto it=boundary_hfaces->begin(boundary_hfaces);it.it!=NULL;it++)
    {
        template_hf* hf=(template_hf*)(it.second);
        template_v* vs[3]={ hf->vertices[0],hf->vertices[1],n_v};
        
        printf("once\n");
        m->create_cellv(m,vs,3);
    } 

   // printf("hrere777777777777777\n");
//update tree_e
    

    printf("hrere8888888\n"); 
    for(auto vfit=m->vf_begin(m,*n_v);vfit!=m->vf_end(m,*n_v);vfit++)
    {
        double *p=compute_qem_one_edge_position(m,quote(vfit),tree_v);
        tree_e->insert(tree_e,quote(vfit)->id,p); 
    } 

//update tree_cost 
    printf("update tree_cost\n");
    for(auto vfit=m->vf_begin(m,*n_v);vfit!=m->vf_end(m,*n_v);vfit++)
    {
        if(is_edge_legacy_to_collapse(m,quote(vfit))==2)
        {
            double cost=qem_compute_one_edge_cost(quote(vfit),tree_e,tree_v);
            //(*tree_cost)[cost]=quote(vfit)->id; 
           
        }  
    }
    printf("update end tree_cost\n");
    int_rb_tree_free(faces);
    int_rb_tree_free(boundary_hfaces);
    int_rb_tree_free(cells);
    m->delete_vertex(m,*v1,1);
    m->delete_vertex(m,*v2,1); 
    printf("end quem\n");

}

template_f* qem_get_first_face(Mesh* m,map<double,int>* tree_cost )
{


    auto  it=tree_cost->begin();

    if(it==tree_cost->end())
    {
        return NULL;
    }
  
   
    template_f* f= m->get_facep(m,it->second);

    while(f==NULL)
    {
        if(it==tree_cost->end())
        {
            return NULL;
        }
        tree_cost->erase(it);
        it=tree_cost->begin(); 
        f=m->get_facep(m,it->second);
    } 
    return f;

}

void my_test()
{

    Mesh mesh;
    mesh.manifold_require=1;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"bone_scan1.off",3); 
    Int_RB_Tree* tree_v= qem_init_qi(&mesh);
    Int_RB_Tree* tree_e=compute_qem_all_edge_position(&mesh,tree_v);
    map<double,int>* tree_cost=qem_get_edge_collapse_cost(&mesh,tree_e,tree_v);
    template_f* f=qem_get_first_face(&mesh,tree_cost);
    printf("num f:%d num_c:%d\n",mesh.num_f(&mesh),mesh.num_c(&mesh));
    for(int i=0;i<4000;i++)
    {
        printf("f id %d %d %d %d %d\n",f->id,i,tree_v->size,tree_cost->size(),tree_e->size);
        qem_collapse_edge(&mesh,f,tree_v,tree_e,tree_cost);
        printf("once\n");
        f=qem_get_first_face(&mesh,tree_cost); 
    }
    //printf("num f:%d\n",mesh.num_f(&mesh));
    printf("num f:%d num_c:%d\n",mesh.num_f(&mesh),mesh.num_c(&mesh));
//可视化
    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);

    Viewer_World *vw=vwm.create_world(&vwm,NULL);

    add_default_somethings(vw);
    //test_show_mesh_lines(vw,&mesh);
    test_show_mesh_cells(vw,&mesh);
    Node* n=vw->create_something(vw,"Edges");
    Viewer_Something* vs=(Viewer_Something*)(n->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    ve->Data_index_rows=1;
    ve->Data_rows=2;
    viewer_tools_fill_data("%f",ve->Data_rows*3,&(ve->Data),
        f->vertices[0]->point[0],f->vertices[0]->point[1],f->vertices[0]->point[2],
        f->vertices[1]->point[0],f->vertices[1]->point[1],f->vertices[1]->point[2]
        );
    viewer_tools_fill_data("%u",ve->Data_index_rows*2,&(ve->Data_index),
        0,1);
    ve->edgesize=10.0;

    // ve->Data=(float*)malloc(sizeof(float)*3*ve->Data_rows);
    // ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    // ve->Data[0]=f->vertices[0]->point[0];ve->Data[1]=f->vertices[0]->point[1]; ve->Data[2]=f->vertices[0]->point[2];
    // ve->Data[3]=f->vertices[1]->point[0];ve->Data[4]=f->vertices[1]->point[1]; ve->Data[5]=f->vertices[1]->point[2];
    // ve->Data_index[0]=0;ve->Data_index[1]=1;
    ve->color_rows=ve->Data_index_rows;
    ve->set_color(ve,1.0,0.0,0.0,1.0);

    Viewer_Opengl_Interpreter voi;
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);
    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm); 

//可视化
    Mesh_free(&mesh);
}



int main()
{
    my_test();
    // double p1[3]={-1,0,0},p2[3]={-1,1,0},p3[3]={-1,0,1};
    // double* plane=get_triangle_plane_equation(p1,p2,p3);
    // printf("%lf %lf %lf %lf\n",plane[0],plane[1],plane[2],plane[3]);
    // Mesh mesh;
    // Mesh_init(&mesh);
    // _ReadOff_(&mesh,"BlueLightPointCloud.off",3);
    // clock_t t=clock();
    // create_kd_tree_from_mesh(&mesh);  
    // printf("运行时间%.4f秒:%d\n", (double)(clock() - t) / CLOCKS_PER_SEC,0); 
    // Mesh_free(&mesh);

    // map<double,int> *tree_cost=new map<double,int>;
    // for(int i=0;i<9999999;i++)
    // {
    //     (*tree_cost)[i]=i;
    // }
    // delete(tree_cost);

    printf("end\n");
    return 0;
}


